#include "cartographer/rcm/imu/filter.h"
#include "cartographer/rcm/imu/options.h"

namespace cartographer
{
    ImuExponentialFilter::ImuExponentialFilter(double threshTime)
        : threshTime_(threshTime)
    {
        auto &options = ImuOptionsFilter::GetInstance();
        filterGyro_ = options.filterGyro;
        intervalMax_ = options.intervalMax;
        double accHorizon = options.threshAccHorizon;
        double gyroHorizon = options.threshGyroHorizon;
        double verticalRatio = options.threshVerticalRatio;
        threshAcc_ = Eigen::Vector3d(accHorizon, accHorizon, verticalRatio * accHorizon);
        threshGyro_ = Eigen::Vector3d(gyroHorizon, gyroHorizon, verticalRatio * gyroHorizon);
    }

    ImuMeasure ImuExponentialFilter::Estimate(const ImuMeasure &last, const ImuMeasure &curr)
    {
        if (curr.time <= last.time)
        {
            LOG(INFO) << "[Filter] time disorder curr " << curr.time << " last " << last.time;
            return curr;
        }
        double duration = common::ToSeconds(curr.time - last.time);
        Eigen::Vector3d angular;
        Eigen::Vector3d acceleration;
        // LOG(INFO) << "[Filter] " << curr.time << " duration " << duration;
        if (duration < intervalMax_)
        {
            Eigen::Vector3d alpha_acc;
            Eigen::Vector3d alpha_gyro;
            Eigen::Vector3d identity(1.0, 1.0, 1.0);
            const auto delta_ang = curr.angular - last.angular;
            const auto delta_acc = curr.acceleration - last.acceleration;
            for (int i = 0; i < 3; i++)
            {
                // std::exp内的值为-0.693147时，alpha=0.5
                alpha_acc(i) = 1.0 - std::exp(-std::fabs(delta_acc(i)) / threshAcc_(i));
                alpha_gyro(i) = 1.0 - std::exp(-std::fabs(delta_ang(i)) / threshGyro_(i));
            }
            for (int i = 0; i < 3; i++)
            {
                // thresh越大，1/e的幂越小，std::exp的结果越大，alpha越小，越相信当前
                // delta越大，1/e的幂越大，std::exp的结果越小，alpha越大，越相信上时刻值
                angular(i) = (identity - alpha_gyro)(i)*curr.angular(i) + alpha_gyro(i) * last.angular(i);
                acceleration(i) = (identity - alpha_acc)(i)*curr.acceleration(i) + alpha_acc(i) * last.acceleration(i);
            }
            // LOG(INFO) << "delta ang:" << VecToStr(delta_ang) << " acc:" << VecToStr(delta_acc);
            // LOG(INFO) << "alpha ang:" << VecToStr(alpha_gyro) << " acc:" << VecToStr(alpha_acc);
            // LOG(INFO) << "org ang:" << VecToStr(curr.angular) << " acc:" << VecToStr(curr.acceleration);
            // LOG(INFO) << "new ang:" << VecToStr(angular) << " acc:" << VecToStr(acceleration);
        }
        else
        {
            const double alpha = 1.0 - std::exp(-duration / threshTime_);
            angular = (1. - alpha) * last.angular + alpha * curr.angular;
            acceleration = (1. - alpha) * last.acceleration + alpha * curr.acceleration;
        }
        // LOG(INFO) << "deltaAfter ang:" << VecToStr(angular - curr.angular) << " acc:" << VecToStr(acceleration - curr.acceleration);
        return ImuMeasure(acceleration, filterGyro_ ? angular : curr.angular, curr.time);
    }

    LMSFilter::LMSFilter(size_t size, double stride)
        : size_(size)
        , stride_(stride)
        , weights(size, 0.0)
    {
    }

    bool LMSFilter::Update(const std::vector<double> &inputs, const double expect, double &output)
    {
        if (inputs.size() < size_)
        {
            std::cerr << "Error: Input signal is too short for the filter order." << std::endl;
            return false;
        }
        output = 0.0;
        for (size_t i = 0; i < size_; ++i)
            output += weights[i] * inputs[i];
        double error = expect - output;
        for (size_t i = 0; i < size_; ++i)
            weights[i] += stride_ * 2 * error * inputs[i];
        return true;
    }

} // namespace cartographer